

#include "lib_shake.h"

#define USING_ANGLE 
#ifdef USING_ANGLE
#include "imu.h"
#endif

#define SH3201_CHIP_ID_REG				(0x1F)
#define SH3201_CHIP_ID_VAL   (0X63)

#define  MAX_CACHE_SIZE 32  // 滑动窗口最大大小
static uint8_t win_size = 10;   // 滑动窗口大小，用于平滑数据
static uint32_t total_time = 0;
static uint8_t init_flag = 0;
static uint8_t b_shake = 0;
short cache[MAX_CACHE_SIZE];
static short shake_thres = -400;
static uint16_t brake_duration= 0;
static uint16_t brake_min_duration= 50;
static short shake_speed = 0;
static uint8_t m_axis = 0;
static IMU_read  imu_read = 0;
static uint8_t chip_valid = 0;

static float euler_angle[3];
static float lean = 60;


int window_filter(short v)
{
    int sum = 0;
    uint8_t i;
    for (i = 0; i < win_size; i++) {
        sum += v;
    }
    return sum / win_size;
}

uint8_t shake_update(short acc[3], short gyro[3], short dt, struct shake_info *info)
{
    int value;
    int rv = 0;
    uint8_t flag1, flag2;
    if(!chip_valid) return 0;
#ifdef USING_ANGLE
    IMU_update(acc, gyro, dt / 1000.0f, euler_angle);
#endif
    total_time += dt;
    if(init_flag  < win_size){
        init_flag += 1;
         info->level = SHAKE_NONE;
        return 0;
    }
    value = window_filter(acc[m_axis]);
    if(value > 0) value = - value;
    if(!b_shake && value < shake_thres){
        b_shake =1;
        shake_speed = value;
    }else{
        if(value < shake_thres){
            brake_duration += dt;
            if(value < shake_speed){
                shake_speed = value;
            }
        }else {
            if(brake_duration >= brake_min_duration) {
                rv = 1;
                info->start = total_time - brake_duration;
                info->duration = brake_duration;
                // TODO: check level
                flag1 = brake_duration  < brake_min_duration * 2 ;  
                flag2 = (shake_speed ) < (shake_thres + (shake_thres/2));
                if(flag1 &&  flag2 ){
                    info->level = SHAKE_L3; 
                }else if(flag1 || flag2){
                    info->level = SHAKE_L2; 
                }else {
                    info->level = SHAKE_L1;
                }
                
                info->speed = shake_speed;
            }
            brake_duration = 0;
            b_shake = 0;
        }
    }
    return rv;
}

uint8_t shake_init(struct shake_param *p)
{
    uint8_t i;
    uint8_t v;
    if(p->win_size > 0 && p->win_size < MAX_CACHE_SIZE){
            win_size = p->win_size;
    }
    brake_min_duration = p->min_duration;
    shake_thres = p->thres; 
    m_axis = p->m_axis;
    imu_read = p->read;
    //
    if(imu_read != 0){
      for(i=0; i<3; i++){
          imu_read(SH3201_CHIP_ID_REG, 1, &v);
          if(v == SH3201_CHIP_ID_VAL){
            chip_valid = 1; break;
          }
      }
    }
    init_flag = 0;
    
    return chip_valid;
}


uint8_t lean_state(void)
{
    return euler_angle[0] > lean || euler_angle[0] < -lean;
}

void get_angle(float angle[3])
{
    angle[0] = euler_angle[0];
    angle[1] = euler_angle[1];
    angle[2] = euler_angle[2];
}

